//
// Created by 李家豪 on 2019/6/5.
//

#ifndef UNIWII_REFERENCEFRAMESOLVER_H
#define UNIWII_REFERENCEFRAMESOLVER_H

#include "stdint.h"

#define DEG_TO_RAD 0.0174532925f
#define RAD_TO_DEG 57.295779513f

// This struct is readonly to user code
// The driver code is in ReferenceFrameSolve.c
typedef struct {
	volatile float capturedAngle; // in radius, converted from att_t::heading
	volatile int16_t capturedAngle16; // in mill-degrees, captured from att_t::heading
	volatile float currentAngle; // in radius, converted from att_t::heading
	volatile int16_t angleDelta16; // = currentAngle - capturedAngle
	volatile float angleDelta; // radius version of angleDelta16
	volatile float rollDelta; // in radius, converted from att_t::angle
	volatile float pitchDelta; // in radius, converted from att_t::angle
	uint8_t captured; // boolean, mark when the reference frame is captured

	uint64_t lastSamplingTime; // machine timestamp with granularity of ms
	uint64_t currentSamplingTime; // machine timestamp with granularity of ms
	int32_t offsetX; // move in the X coordinate (left-right) with granularity of cm
	int32_t offsetY; // move in the Y coordinate (forward-backward) with granularity of cm
	int32_t offsetZ; // move in the Z coordinate (up-down) with granularity of cm
} frame_t;

extern frame_t frame;

extern void frame_capture();

extern void frame_reset();

extern void frame_sample();

extern void frame_offset_calculate();

extern void frame_2d16_from_reference(int16_t *x, int16_t *y);

extern void frame_2d16_from_base(int16_t *x, int16_t *y);

#endif //UNIWII_REFERENCEFRAMESOLVER_H
